/*
The MIT License
Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE

This file is part of embeddedRTPS.

Author: i11 - Embedded Software, RWTH Aachen University
*/

#include "rtps/messages/MessageReceiver.h"
#include <rtps/entities/Participant.h>

#include "rtps/entities/Reader.h"
#include "rtps/entities/Writer.h"
#include "rtps/messages/MessageTypes.h"
#include "rtps/utils/Log.h"
#include "rtps/utils/printutils.h"

#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE
#define RECV_LOG(...)                                                          \
  if (true) {                                                                  \
    printf("[RECV] ");                                                         \
    printf(__VA_ARGS__);                                                       \
    printf("\n");                                                              \
  }
#else
#define RECV_LOG(...) //
#endif

void MessageReceiver_construct(struct rtps_Participant *part, struct MessageReceiver *receiver)
{
    receiver->mp_part = part;
}

void rtps_MessageReceiver_resetState(struct MessageReceiver *receiver) 
{
  receiver->sourceGuidPrefix = GUIDPREFIX_UNKNOWN;
  receiver->sourceVersion = PROTOCOLVERSION;
  receiver->sourceVendor = VENDOR_UNKNOWN;
  receiver->haveTimeStamp = false;
}

bool rtps_MessageReceiver_processMessage(uint8_t *data, DataSize_t size, struct MessageReceiver *receiver)
{
  rtps_MessageReceiver_resetState(receiver);
  struct MessageProcessingInfo msgInfo;

  msgInfo.data = data;
  msgInfo.size = size;
  msgInfo.nextPos = 0;

  if (!rtps_MessageReceiver_processHeader(&msgInfo, receiver)) {
    return false;
  }
  struct SubmessageHeader submsgHeader;
  while (msgInfo.nextPos < msgInfo.size) {
    if (!deserializeMessageSubmessageHeader(&msgInfo, &submsgHeader)) {
      return false;
    }
    rtps_MessageReceiver_processSubmessage(&msgInfo, &submsgHeader, receiver);
  }

  return true;
}

bool rtps_MessageReceiver_processHeader(struct MessageProcessingInfo *msgInfo, struct MessageReceiver *receiver)
{
  struct Header header = {0};

  if (!deserializeMessageHeader(msgInfo, &header)) {
    return false;
  }

  if (rtps_GuidPrefix_t_operator_equal(&header.guidPrefix, &receiver->mp_part->m_guidPrefix)) {
    RECV_LOG("[MessageReceiver]: Received own message.\n");
    return false; // Don't process our own packet
  }
  
  if (memcmp(header.protocolName, RTPS_PROTOCOL_NAME, sizeof(header.protocolName)) ||
      header.protocolVersion.major != PROTOCOLVERSION.major) {
    return false;
  }

  receiver->sourceGuidPrefix = header.guidPrefix;
  receiver->sourceVendor = header.vendorId;
  receiver->sourceVersion = header.protocolVersion;

  msgInfo->nextPos += HeaderGetRawSize();
  return true;
}

bool rtps_MessageReceiver_processSubmessage(struct MessageProcessingInfo *msgInfo, const struct SubmessageHeader *submsgHeader, struct MessageReceiver *receiver)
{
  bool success = false;

  switch (submsgHeader->submessageId) {
  case ACKNACK:
    RECV_LOG("Processing AckNack submessage\n");
    success = rtps_MessageReceiver_processAckNackSubmessage(msgInfo, receiver);
    break;
  case DATA:
    RECV_LOG("Processing Data submessage\n");
    success = rtps_MessageReceiver_processDataSubmessage(msgInfo, submsgHeader, receiver);
    break;
  case HEARTBEAT:
    RECV_LOG("Processing Heartbeat submessage\n");
    success = rtps_MessageReceiver_processHeartbeatSubmessage(msgInfo, receiver);
    break;
  case INFO_DST:
    RECV_LOG("Info_DST submessage not relevant.\n");
    success = true; // Not relevant
    break;
  case INFO_TS:
    RECV_LOG("Info_TS submessage not relevant.\n");
    success = true; // Not relevant now
    break;
  default:
    RECV_LOG("Submessage of type %u currently not supported. Skipping..\n",
             (uint8_t)(submsgHeader->submessageId));
    success = false;
  }
  msgInfo->nextPos +=
      submsgHeader->octetsToNextHeader + SubmessageHeaderGetRawSize();
  return success;
}

bool rtps_MessageReceiver_processDataSubmessage(struct MessageProcessingInfo *msgInfo, const struct SubmessageHeader *submsgHeader, struct MessageReceiver *receiver)
{
  struct SubmessageData dataSubmsg;

  if (!deserializeMessageSubmessageData(msgInfo, &dataSubmsg)) {
    return false;
  }

  uint8_t *serializedData = getPointerToCurrentPos(msgInfo) + SubmessageDataGetRawSize();

  const DataSize_t size = submsgHeader->octetsToNextHeader -
                          SubmessageDataGetRawSize() +
                          SubmessageHeaderGetRawSize();

  RECV_LOG("Received data message size %u", (int)size);

  Reader *reader;
  Guid_t guid;
  guid.prefix = receiver->sourceGuidPrefix;
  guid.entityId = dataSubmsg.writerId;
  if (!memcmp(&dataSubmsg.readerId, &ENTITYID_UNKNOWN, sizeof(EntityId_t))) {
#if RECV_VERBOSE
    RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = ");
    printGuid(guid);
    printf("\n");
#endif

    reader = rtps_Participant_getReaderByWriterId(receiver->mp_part, &guid);
    if (reader != NULL) {
      RECV_LOG("Found reader!");
    }
  } else {
    reader = rtps_Participant_getReader(receiver->mp_part, dataSubmsg.readerId);
#if RECV_VERBOSE
    Reader *reader_by_writer = rtps_Participant_getReaderByWriterId(receiver->mp_part, &guid);

    if (reader_by_writer == NULL && reader != NULL) {
      RECV_LOG("FOUND By READER ID, NOT BY WRITER ID =");
      printGuid(guid);
      printf("\n");
    }
#endif
  }
  if (reader != NULL) {
    Guid_t writerGuid;
    writerGuid.prefix = receiver->sourceGuidPrefix;
    writerGuid.entityId = dataSubmsg.writerId;
    ReaderCacheChange change;
    rtps_ReaderCacheChange_Construct(&change, ALIVE, &writerGuid, dataSubmsg.writerSN, 
        serializedData, size);
    if (reader->m_bstateful)
        rtps_StatefulReader_newChange((struct rtps_StatefulReader *)reader, &change);
    else
        rtps_StatelessReader_newChange((struct rtps_StatelessReader *)reader, &change);
  } else {
#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE
    RECV_LOG("Couldn't find a reader with id: ");
    printEntityId(dataSubmsg.readerId);
    printf("\n");
#endif
  }

  return true;
}

bool rtps_MessageReceiver_processHeartbeatSubmessage(struct MessageProcessingInfo *msgInfo, struct MessageReceiver *receiver) 
{
  struct SubmessageHeartbeat submsgHB;
  if (!deserializeMessageSubmessageHeartbeat(msgInfo, &submsgHB)) {
    return false;
  }

  Reader *reader = rtps_Participant_getReader(receiver->mp_part, submsgHB.readerId);
  if (reader != NULL) {
    if (reader->m_bstateful)
      rtps_StatefulReader_onNewHeartbeat((struct rtps_StatefulReader *)reader, &submsgHB, &receiver->sourceGuidPrefix);
    else
      rtps_StatelessReader_onNewHeartbeat((struct rtps_StatelessReader *)reader, &submsgHB, &receiver->sourceGuidPrefix);
   
    rtps_Participant_addHeartbeat(receiver->mp_part, receiver->sourceGuidPrefix);
    return true;
  } else {
    return false;
  }
}

bool rtps_MessageReceiver_processAckNackSubmessage(struct MessageProcessingInfo *msgInfo, struct MessageReceiver *receiver)
{
  struct SubmessageAckNack submsgAckNack;
  if (!deserializeMessageSubmessageAckNack(msgInfo, &submsgAckNack)) {
    return false;
  }

  struct rtps_Writer *writer = rtps_Participant_getWriter(receiver->mp_part, submsgAckNack.writerId);
  if (writer != NULL) {
    if (writer->m_bstateful)
      rtps_StatefulWriter_onNewAckNack((struct rtps_StatefulWriter *)writer, &submsgAckNack, &(receiver->sourceGuidPrefix));
    else
      rtps_StatelessWriter_onNewAckNack((struct rtps_StatelessWriter *)writer, &submsgAckNack, &(receiver->sourceGuidPrefix));    
    return true;
  } else {
    return false;
  }
}

#undef RECV_VERBOSE
